use r2r::unitree_api::msg::Request;
use std::thread::sleep;
use std::time::Duration;
use unitree_ros2_rs::obsavoid_client::client::ObsAvoidClient;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    let obsavoid_client = ObsAvoidClient::new("obsavoid_demo", "")?;

    let mut req = Request::default();

    println!("Starting obstacle avoidance demo...");

    // 开启避障
    println!("Enabling obstacle avoidance...");
    obsavoid_client.set_enable(&mut req, true);
    sleep(Duration::from_secs(5));

    // 关闭避障
    println!("Disabling obstacle avoidance...");
    obsavoid_client.set_enable(&mut req, false);
    sleep(Duration::from_secs(5));

    // 开启避障
    println!("Enabling obstacle avoidance again...");
    obsavoid_client.set_enable(&mut req, true);
    sleep(Duration::from_secs(5));

    println!("Obstacle avoidance demo finished.");

    Ok(())
}